home *** CD-ROM | disk | FTP | other *** search
/ Amiga Plus 2002 #11 / Amiga Plus CD - 2002 - No. 11.iso / Tools / Development / libogg / libvorbis-1.0rc3 / lib / iir.c < prev    next >
Encoding:
C/C++ Source or Header  |  2002-10-27  |  6.8 KB  |  301 lines
  1. /********************************************************************
  2.  *                                                                  *
  3.  * THIS FILE IS PART OF THE OggVorbis SOFTWARE CODEC SOURCE CODE.   *
  4.  * USE, DISTRIBUTION AND REPRODUCTION OF THIS LIBRARY SOURCE IS     *
  5.  * GOVERNED BY A BSD-STYLE SOURCE LICENSE INCLUDED WITH THIS SOURCE *
  6.  * IN 'COPYING'. PLEASE READ THESE TERMS BEFORE DISTRIBUTING.       *
  7.  *                                                                  *
  8.  * THE OggVorbis SOURCE CODE IS (C) COPYRIGHT 1994-2001             *
  9.  * by the XIPHOPHORUS Company http://www.xiph.org/                  *
  10.  *                                                                  *
  11.  ********************************************************************
  12.  
  13.   function: Direct Form II IIR filters, plus some specializations
  14.   last mod: $Id: iir.c,v 1.12 2001/12/20 01:00:26 segher Exp $
  15.  
  16.  ********************************************************************/
  17.  
  18. /* LPC is actually a degenerate case of form I/II filters, but we need
  19.    both */
  20.  
  21. #include <ogg/ogg.h>
  22. #include <stdlib.h>
  23. #include <string.h>
  24. #include <math.h>
  25. #include "iir.h"
  26.  
  27. void IIR_init(IIR_state *s,int stages,float gain, float *A, float *B){
  28.   memset(s,0,sizeof(*s));
  29.   s->stages=stages;
  30.   s->gain=1.f/gain;
  31.   s->coeff_A=_ogg_malloc(stages*sizeof(*s->coeff_A));
  32.   s->coeff_B=_ogg_malloc((stages+1)*sizeof(*s->coeff_B));
  33.   s->z_A=_ogg_calloc(stages*2,sizeof(*s->z_A));
  34.  
  35.   memcpy(s->coeff_A,A,stages*sizeof(*s->coeff_A));
  36.   memcpy(s->coeff_B,B,(stages+1)*sizeof(*s->coeff_B));
  37. }
  38.  
  39. void IIR_clear(IIR_state *s){
  40.   if(s){
  41.     _ogg_free(s->coeff_A);
  42.     _ogg_free(s->coeff_B);
  43.     _ogg_free(s->z_A);
  44.     memset(s,0,sizeof(*s));
  45.   }
  46. }
  47.  
  48. void IIR_reset(IIR_state *s){
  49.   memset(s->z_A,0,sizeof(*s->z_A)*s->stages*2);
  50. }
  51.  
  52. float IIR_filter(IIR_state *s,float in){
  53.   int stages=s->stages,i;
  54.   float newA= in*s->gain;
  55.   float newB=0;
  56.   float *zA=s->z_A+s->ring;
  57.  
  58.   for(i=0;i<stages;i++){
  59.     newA+= s->coeff_A[i] * zA[i];
  60.     newB+= s->coeff_B[i] * zA[i];
  61.   }
  62.   newB+=newA*s->coeff_B[stages];
  63.  
  64.   zA[0]=zA[stages]=newA;
  65.   if(++s->ring>=stages)s->ring=0;
  66.   return(newB);
  67. }
  68.  
  69. /* this assumes the symmetrical structure of the feed-forward stage of
  70.    a typical bandpass to save multiplies */
  71. float IIR_filter_Band(IIR_state *s,float in){
  72.   int stages=s->stages,i;
  73.   int stages2=stages>>1;
  74.   float newA= in*s->gain;
  75.   float newB=0;
  76.   float *zA=s->z_A+s->ring;
  77.  
  78.   newA+= s->coeff_A[0] * zA[0];
  79.   for(i=1;i<stages2;i++){
  80.     newA+= s->coeff_A[i] * zA[i];
  81.     newB+= s->coeff_B[i] * (zA[i]-zA[stages-i]);
  82.   }
  83.   newB+= s->coeff_B[i] * zA[i];
  84.   for(;i<stages;i++)
  85.     newA+= s->coeff_A[i] * zA[i];
  86.  
  87.   newB+=newA-zA[0];
  88.  
  89.   zA[0]=zA[stages]=newA;
  90.   if(++s->ring>=stages)s->ring=0;
  91.   return(newB);
  92. }
  93.  
  94. #ifdef _V_SELFTEST
  95.  
  96. /* z^-stage, z^-stage+1... */
  97. static float cheb_bandpass_B[]={-1.f,0.f,5.f,0.f,-10.f,0.f,10.f,0.f,-5.f,0.f,1f};
  98. static float cheb_bandpass_A[]={-0.6665900311f,
  99.                   1.0070146601f,
  100.                  -3.1262875409f,
  101.                    3.5017171569f,
  102.                  -6.2779211945f,
  103.                   5.2966481740f,
  104.                  -6.7570216587f,
  105.                   4.0760335768f,
  106.                  -3.9134284363f,
  107.                   1.3997338886f};
  108.  
  109. static float data[128]={  
  110.   0.0426331f,
  111.   0.0384521f,
  112.   0.0345764f,
  113.   0.0346069f,
  114.   0.0314636f,
  115.   0.0310059f,
  116.   0.0318604f,
  117.   0.0336304f,
  118.   0.036438f,
  119.   0.0348511f,
  120.   0.0354919f,
  121.   0.0343628f,
  122.   0.0325623f,
  123.   0.0318909f,
  124.   0.0263367f,
  125.   0.0225525f,
  126.   0.0195618f,
  127.   0.0160828f,
  128.   0.0168762f,
  129.   0.0145569f,
  130.   0.0126343f,
  131.   0.0127258f,
  132.   0.00820923f,
  133.   0.00787354f,
  134.   0.00558472f,
  135.   0.00204468f,
  136.   3.05176e-05f,
  137.   -0.00357056f,
  138.   -0.00570679f,
  139.   -0.00991821f,
  140.   -0.0101013f,
  141.   -0.00881958f,
  142.   -0.0108948f,
  143.   -0.0110168f,
  144.   -0.0119324f,
  145.   -0.0161438f,
  146.   -0.0194702f,
  147.   -0.0229187f,
  148.   -0.0260315f,
  149.   -0.0282288f,
  150.   -0.0306091f,
  151.   -0.0330505f,
  152.   -0.0364685f,
  153.   -0.0385742f,
  154.   -0.0428772f,
  155.   -0.043457f,
  156.   -0.0425415f,
  157.   -0.0462341f,
  158.   -0.0467529f,
  159.   -0.0489807f,
  160.   -0.0520325f,
  161.   -0.0558167f,
  162.   -0.0596924f,
  163.   -0.0591431f,
  164.   -0.0612793f,
  165.   -0.0618591f,
  166.   -0.0615845f,
  167.   -0.0634155f,
  168.   -0.0639648f,
  169.   -0.0683594f,
  170.   -0.0718079f,
  171.   -0.0729675f,
  172.   -0.0791931f,
  173.   -0.0860901f,
  174.   -0.0885315f,
  175.   -0.088623f,
  176.   -0.089386f,
  177.   -0.0899353f,
  178.   -0.0886841f,
  179.   -0.0910645f,
  180.   -0.0948181f,
  181.   -0.0919495f,
  182.   -0.0891418f,
  183.   -0.0916443f,
  184.   -0.096344f,
  185.   -0.100464f,
  186.   -0.105499f,
  187.   -0.108612f,
  188.   -0.112213f,
  189.   -0.117676f,
  190.   -0.120911f,
  191.   -0.124329f,
  192.   -0.122162f,
  193.   -0.120605f,
  194.   -0.12326f,
  195.   -0.12619f,
  196.   -0.128998f,
  197.   -0.13205f,
  198.   -0.134247f,
  199.   -0.137939f,
  200.   -0.143555f,
  201.   -0.14389f,
  202.   -0.14859f,
  203.   -0.153717f,
  204.   -0.159851f,
  205.   -0.164551f,
  206.   -0.162811f,
  207.   -0.164276f,
  208.   -0.156952f,
  209.   -0.140564f,
  210.   -0.123291f,
  211.   -0.10321f,
  212.   -0.0827637f,
  213.   -0.0652466f,
  214.   -0.053772f,
  215.   -0.0509949f,
  216.   -0.0577698f,
  217.   -0.0818176f,
  218.   -0.114929f,
  219.   -0.148895f,
  220.   -0.181122f,
  221.   -0.200714f,
  222.   -0.21048f,
  223.   -0.203644f,
  224.   -0.179413f,
  225.   -0.145325f,
  226.   -0.104492f,
  227.   -0.0658264f,
  228.   -0.0332031f,
  229.   -0.0106201f,
  230.   -0.00363159f,
  231.   -0.00909424f,
  232.   -0.0244141f,
  233.   -0.0422058f,
  234.   -0.0537415f,
  235.   -0.0610046f,
  236.   -0.0609741f,
  237.   -0.0547791f};
  238.  
  239. /* comparison test code from http://www-users.cs.york.ac.uk/~fisher/mkfilter/
  240.    (the above page kicks ass, BTW)*/
  241.  
  242. #define NZEROS 10
  243. #define NPOLES 10
  244. #define GAIN   4.599477515e+02f
  245.  
  246. static float xv[NZEROS+1], yv[NPOLES+1];
  247.  
  248. static float filterloop(float next){ 
  249.   xv[0] = xv[1]; xv[1] = xv[2]; xv[2] = xv[3]; xv[3] = xv[4]; xv[4] = xv[5]; 
  250.   xv[5] = xv[6]; xv[6] = xv[7]; xv[7] = xv[8]; xv[8] = xv[9]; xv[9] = xv[10]; 
  251.   xv[10] = next / GAIN;
  252.   yv[0] = yv[1]; yv[1] = yv[2]; yv[2] = yv[3]; yv[3] = yv[4]; yv[4] = yv[5]; 
  253.   yv[5] = yv[6]; yv[6] = yv[7]; yv[7] = yv[8]; yv[8] = yv[9]; yv[9] = yv[10]; 
  254.   yv[10] =   (xv[10] - xv[0]) + 5 * (xv[2] - xv[8]) + 10 * (xv[6] - xv[4])
  255.     + ( -0.6665900311f * yv[0]) + (  1.0070146601f * yv[1])
  256.     + ( -3.1262875409f * yv[2]) + (  3.5017171569f * yv[3])
  257.     + ( -6.2779211945f * yv[4]) + (  5.2966481740f * yv[5])
  258.     + ( -6.7570216587f * yv[6]) + (  4.0760335768f * yv[7])
  259.     + ( -3.9134284363f * yv[8]) + (  1.3997338886f * yv[9]);
  260.   return(yv[10]);
  261. }
  262.  
  263. #include <stdio.h>
  264. int main(){
  265.  
  266.   /* run the pregenerated Chebyshev filter, then our own distillation
  267.      through the generic and specialized code */
  268.   float *work=_ogg_malloc(128*sizeof(*work));
  269.   IIR_state iir;
  270.   int i;
  271.  
  272.   for(i=0;i<128;i++)work[i]=filterloop(data[i]);
  273.   {
  274.     FILE *out=fopen("IIR_ref.m","w");
  275.     for(i=0;i<128;i++)fprintf(out,"%g\n",work[i]);
  276.     fclose(out);
  277.   }
  278.  
  279.   IIR_init(&iir,NPOLES,GAIN,cheb_bandpass_A,cheb_bandpass_B);
  280.   for(i=0;i<128;i++)work[i]=IIR_filter(&iir,data[i]);
  281.   {
  282.     FILE *out=fopen("IIR_gen.m","w");
  283.     for(i=0;i<128;i++)fprintf(out,"%g\n",work[i]);
  284.     fclose(out);
  285.   }
  286.   IIR_clear(&iir);  
  287.  
  288.   IIR_init(&iir,NPOLES,GAIN,cheb_bandpass_A,cheb_bandpass_B);
  289.   for(i=0;i<128;i++)work[i]=IIR_filter_ChebBand(&iir,data[i]);
  290.   {
  291.     FILE *out=fopen("IIR_cheb.m","w");
  292.     for(i=0;i<128;i++)fprintf(out,"%g\n",work[i]);
  293.     fclose(out);
  294.   }
  295.   IIR_clear(&iir);  
  296.  
  297.   return(0);
  298. }
  299.  
  300. #endif
  301.